#include <a02_task3/car_state.h>
#include <ros/ros.h>

#include <iostream>

#include "ugv_sdk/scout/scout_base.hpp"
#include "ugv_sdk/scout/scout_types.hpp"

using namespace westonrobot;

a02_task3::car_state CutOutState(ScoutState state, int count);

int main(int argc, char *argv[]) {
  ros::init(argc, argv, "car_state_publisher");
  ros::NodeHandle nh;
  ros::Publisher pub =
      nh.advertise<a02_task3::car_state>("car_state", 1000);
  // 0.1 s 一次
  ros::Rate loop_rate(10);
  int count = 0;

  std::string device_name;
  int32_t baud_rate = 0;

  if (argc == 2) {
    device_name = {argv[1]};
    std::cout << "Specified CAN: " << device_name << std::endl;
  } else if (argc == 3) {
    device_name = {argv[1]};
    baud_rate = std::stol(argv[2]);
    std::cout << "Specified serial: " << device_name << "@" << baud_rate
              << std::endl;
  } else {
    std::cout << "Usage: app_scout_demo <interface>" << std::endl
              << "Example 1: ./app_scout_demo can0" << std::endl
              << "Example 2: ./app_scout_demo /dev/ttyUSB0 115200" << std::endl;
    return -1;
  }

  ScoutBase scout;
  scout.Connect(device_name, baud_rate);

  while (ros::ok()) {
    auto state = scout.GetScoutState();
    std::cout << "start get state by sdk and publish info"
              << std::endl;
    std::cout << "linear_velocity: " << state.linear_velocity << "angular_velocity: " << state.angular_velocity << std::endl;
    a02_task3::car_state needed_state = CutOutState(state, count);
    pub.publish(needed_state);
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }
  return 0;
}

a02_task3::car_state CutOutState(ScoutState state, int count) {
  a02_task3::car_state res;
  // control mode 0 is default mode(remote mode). mode 1 is cmd
  // mode(CTRL_MODE_CMD_CAN), 2 is CTRL_MODE_CMD_UART. BASE_STATE_NORMAL,
  // BASE_STATE_ESTOP, BASE_STATE_EXCEPTION
  switch (state.control_mode) {
    case CTRL_MODE_RC:
      res.control_mode = "remote control mode";
      std::cout << "Now is remote control mode." << std::endl;
      break;
    case CTRL_MODE_CMD_CAN:
      res.control_mode = "CAN command control mode";
      std::cout << "Now is command control mode." << std::endl;
      break;
    default:
      res.control_mode = "UART command control mode";
      std::cout << "CTRL_MODE_CMD_UART mode" << std::endl;
  }

  // 我不理解，命名未定义的枚举，为什么sdk里可以用
  switch (static_cast<int>(state.base_state)) {
    case 0:
      res.base_state = "BASE_STATE_NORMAL";
      std::cout << "BASE_STATE_NORMAL" << std::endl;
      break;
    case 1:
      res.base_state = "BASE_STATE_ESTOP";
      std::cout << "BASE_STATE_ESTOP" << std::endl;
      break;
    default:
      res.base_state = "BASE_STATE_EXCEPTION";
      std::cout << "BASE_STATE_EXCEPTION" << std::endl;
  }
  // 头
  res.header.seq = count;
  res.header.stamp = ros::Time::now();
  res.header.frame_id = "car_state";

  // 车灯: CONST_OFF 0， CONST_ON 1， BREATH 2， CUSTOM 3
  switch (static_cast<int>(state.front_light_state.mode)) {
    case 0:
      res.front_light_state = "CONST_OFF";
      break;
    case 1:
      res.front_light_state = "CONST_ON";
      break;
    case 2:
      res.front_light_state = "BREATH";
      break;
    case 4:
      res.front_light_state = "CUSTOM";
      break;
    default:
      res.front_light_state = "CONST_OFF";
  }

  // 电池
  res.battery_voltage = state.battery_voltage;
  // 运动状态, 内置有最大的 max_linear_velocity， min_linear_velocity，
  // max_angular_velocity， min_angular_velocity，所以不用自己设置
  res.linear_velocity = state.linear_velocity;
  res.angular_velocity = state.angular_velocity;
  // 里程
  res.left_odometry = state.left_odometry;
  res.right_odometry = state.right_odometry;
  // 电机, 固定四轮
  const int motor_num = 4;
  for (int i = 0; i < motor_num; i++) {
    res.motor_rpm[i] = state.actuator_states[i].motor_rpm;
    res.driver_voltage[i] = state.actuator_states[i].driver_voltage;
    res.motor_temperature[i] = state.actuator_states[i].motor_temperature;
  }
  return res;
}